#include <Windows.h>
#include <string>
#include <iostream>

#include "BrainException.h"
#include "BTChannel.h"


#include "RoomStrategy.h"


CRoomStrategy::CRoomStrategy(void)
{
}


CRoomStrategy::~CRoomStrategy(void)
{
}

void CRoomStrategy::run()
{
	using namespace std;

	try {
		cout << "Starting room strategy" << endl;
		CBTChannel btChannel;
		cout << "Connecting to Tractor" << endl;
		btChannel.connect();
		cout << "Connected OK" << endl;


		while (true) {
			cout << "Setting eye to angle 0" << endl;
			btChannel.setEyeAngle(0);
			cout << "Moving forward" << endl;
			btChannel.forward();

			while (true) {
				int d = btChannel.getDistance();
				cout << "Distance: " << d << endl;
				if (d > 0 && d < 25) {
					break;
				}
				Sleep(10);
			}
			cout << "Wall found - stopping" << endl;
			btChannel.stop();

			cout << "Setting eye to angle 90" << endl;
			btChannel.setEyeAngle(90);
			Sleep(1000);
			int d1 = btChannel.getDistance();
			cout << "Distance: " << d1 << endl;

			cout << "Setting eye to angle -90" << endl;
			btChannel.setEyeAngle(-90);
			Sleep(1000);
			int d2 = btChannel.getDistance();
			cout << "Distance: " << d2 << endl;

			cout << "Setting eye to angle 0" << endl;
			btChannel.setEyeAngle(0);
			Sleep(1000);

			if (d1 > d2) {
				btChannel.right();
				cout << "Turning right..." << endl;
			} else {
				btChannel.left();
				cout << "Turning left..." << endl;
			}

			while (true) {
				int d = btChannel.getDistance();
				cout << "Distance: " << d << endl;
				if (d > 0 && d > 50) {
					break;
				}
			}
			btChannel.stop();
			Sleep(1000);
		}

		cout << "Disconnecting" << endl;
		btChannel.disconnect();

	} catch (BrainException e) {
		cout << "Exception: " << e.getReason() << endl;
		cout << "Stoping" << endl;
	}
}

